#include <iostream>
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include "lidar_tool/param_extinct2Config.h"
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <image_transport/image_transport.h> // 用来发布和订阅图像信息
#include <cv_bridge/cv_bridge.h>             // 将ROS下的sensor_msgs/Image消息类型转化为cv::Mat数据类型

using namespace std;
struct InitialParameters
{
    bool fisheye_model;
    cv::Mat camera_mat; //内参
    cv::Mat dist_coeff;
};
InitialParameters _i_params;

Eigen::Affine3f transform0;
static void Callbackdynamic(lidar_tool::param_extinct2Config &config)
{
    // transform0 = Eigen::Affine3f::Identity();
    // transform0.translation() << config.x, config.y, config.z;
    // transform0.rotate(Eigen::AngleAxisf(config.yaw, Eigen::Vector3f::UnitZ()));
    // transform0.rotate(Eigen::AngleAxisf(config.pitch, Eigen::Vector3f::UnitY()));
    // transform0.rotate(Eigen::AngleAxisf(config.roll, Eigen::Vector3f::UnitX()));

    Eigen::MatrixXf t1(4, 4), t2(4, 4);

    Eigen::Matrix3f rotation_matrix;
    rotation_matrix = Eigen::AngleAxisf(config.yaw, Eigen::Vector3f::UnitZ()) *
                      Eigen::AngleAxisf(config.pitch, Eigen::Vector3f::UnitY()) *
                      Eigen::AngleAxisf(config.roll, Eigen::Vector3f::UnitX());
    t1.matrix() << 0, 0, 0, config.x,
        0, 0, 0, config.y,
        0, 0, 0, config.z,
        0, 0, 0, 1;
    t1.block(0, 0, 3, 3) = rotation_matrix;
    t2 = t1;
    transform0.matrix() << t2(0, 0), t2(0, 1), t2(0, 2), t2(0, 3),
        t2(1, 0), t2(1, 1), t2(1, 2), t2(1, 3),
        t2(2, 0), t2(2, 1), t2(2, 2), t2(2, 3),
        t2(3, 0), t2(3, 1), t2(3, 2), t2(3, 3);
}

ros::NodeHandle &get_handle()
{
    static ros::NodeHandle nh;
    return nh;
}
template <class T>
ros::Publisher &get_publisher(const std::string &topic)
{
    static std::map<std::string, ros::Publisher> publisher_map;
    if (publisher_map.find(topic) == publisher_map.end())
    {
        publisher_map[topic] = get_handle().advertise<T>(topic, 1000);
    }
    return publisher_map[topic];
}

template <typename Point>
void pubpc(std::string topic, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    get_publisher<sensor_msgs::PointCloud2>(topic).publish(msgs);
}

cv::Point2d converto_imgpts(const pcl::PointXYZI &pt)
{
    // 按照线性模型，归一化(f=1)图像坐标系
    double tmpxC = pt.x / pt.z;
    double tmpyC = pt.y / pt.z;
    cv::Point2d planepointsC;

    //    planepointsC.x = tmpxC;
    //    planepointsC.y = tmpyC;

    double r2 = tmpxC * tmpxC + tmpyC * tmpyC; //归一化平面的极坐标轴长的平方

    if (_i_params.fisheye_model)
    {
        double r1 = pow(r2, 0.5);  //归一化平面的极坐标轴长
        double a0 = std::atan(r1); //点和光轴的夹角
        double a1 = a0 * (1 + _i_params.dist_coeff.at<double>(0) * pow(a0, 2) +
                          _i_params.dist_coeff.at<double>(1) * pow(a0, 4) + _i_params.dist_coeff.at<double>(2) * pow(a0, 6) +
                          _i_params.dist_coeff.at<double>(3) * pow(a0, 8)); //畸变矫正，因镜头误差导致的新夹角，根据等距模型，也等于成像平面的矫正后新轴距
        //按照相似三角形计算矫正后的归一化平面点的坐标
        planepointsC.x = (a1 / r1) * tmpxC;
        planepointsC.y = (a1 / r1) * tmpyC;
        //计算像素坐标
        planepointsC.x = _i_params.camera_mat.at<double>(0, 0) * planepointsC.x + _i_params.camera_mat.at<double>(0, 2);
        planepointsC.y = _i_params.camera_mat.at<double>(1, 1) * planepointsC.y + _i_params.camera_mat.at<double>(1, 2);
    }
    else // For pinhole camera model
    {
        double tmpdist = 1 + _i_params.dist_coeff.at<double>(0) * r2 + _i_params.dist_coeff.at<double>(1) * r2 * r2 +
                         _i_params.dist_coeff.at<double>(4) * r2 * r2 * r2;
        planepointsC.x = tmpxC * tmpdist + 2 * _i_params.dist_coeff.at<double>(2) * tmpxC * tmpyC +
                         _i_params.dist_coeff.at<double>(3) * (r2 + 2 * tmpxC * tmpxC);
        planepointsC.y = tmpyC * tmpdist + _i_params.dist_coeff.at<double>(2) * (r2 + 2 * tmpyC * tmpyC) +
                         2 * _i_params.dist_coeff.at<double>(3) * tmpxC * tmpyC;
        planepointsC.x = _i_params.camera_mat.at<double>(0, 0) * planepointsC.x + _i_params.camera_mat.at<double>(0, 2);
        planepointsC.y = _i_params.camera_mat.at<double>(1, 1) * planepointsC.y + _i_params.camera_mat.at<double>(1, 2);
    }
    return planepointsC;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
int main(int argc, char *argv[])
{
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/hw2camera/mid_car/1.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file.pcd\n");
        return (-1);
    }
    cv::Mat image = cv::imread("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/hw2camera/mid_car/1.jpg");

    ros::init(argc, argv, "test_node");
    dynamic_reconfigure::Server<lidar_tool::param_extinct2Config> server;
    dynamic_reconfigure::Server<lidar_tool::param_extinct2Config>::CallbackType f;
    f = boost::bind(&Callbackdynamic, _1); //绑定回调函数
    server.setCallback(f);

    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("/myimg", 1);

    _i_params.fisheye_model = false;
    // _i_params.camera_mat.resize((3, 3));
    // _i_params.camera_mat << 339.632155, 0, 640.99559, 0, 341.455555, 356.35080, 0, 0, 1;
    // _i_params.dist_coeff.resize((1, 4));
    // _i_params.dist_coeff << 0.138878, -0.0364199, 0.000448, 0.0002735;

    if (_i_params.fisheye_model)
    {
        //鱼眼
        double camera_mat[9] = {4.7744114021272674e+02, 0., 9.6438755066918418e+02, 0., 4.7991906955214409e+02, 5.3697598079257989e+02, 0, 0, 1};
        cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(_i_params.camera_mat); //内参
        double dist_coeff[4] = {6.2849888891468336e-02, -2.4029595614050239e-02, 9.9529184047524553e-03, -2.2128438750643269e-03};
        cv::Mat(1, 4, CV_64F, &dist_coeff).copyTo(_i_params.dist_coeff); //畸变系数
    }
    else
    {
        // //小孔
        // double camera_mat[9] = {1949.7533168494813, 0., 996.8739160943692, 0., 1943.351745233424,
        //                         462.66729438851775, 0, 0, 1};
        // cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(_i_params.camera_mat); //内参
        // double dist_coeff[5] = {-0.5434736162932838,
        //                         0.33114769881879574,
        //                         0.0026528927512422837,
        //                         -0.0025526579297368763};
        // cv::Mat(1, 5, CV_64F, &dist_coeff).copyTo(_i_params.dist_coeff); //畸变系数

        //小孔
        double camera_mat[9] = {1949.7533168494813, 0, 996.8739160943692, 0, 1943.351745233424, 462.66729438851775, 0, 0, 1};
        cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(_i_params.camera_mat); //内参
        double dist_coeff[5] = {-0.5434736162932838, 0.33114769881879574, 0.0026528927512422837, -0.0025526579297368763, 0};
        cv::Mat(1, 5, CV_64F, &dist_coeff).copyTo(_i_params.dist_coeff); //畸变系数
    }

    std::cout << "\n内参\n"
              << _i_params.camera_mat << "\n畸变\n"
              << _i_params.dist_coeff << std::endl;

    //     外参
    Eigen::Affine3f transform;
    transform = Eigen::Affine3f::Identity();
    // transform.translation() << 2.36, 0.0, 0.0;
    // transform.rotate(Eigen::AngleAxisf(1.57, Eigen::Vector3f::UnitZ()));
    // transform.rotate(Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitY()));
    // transform.rotate(Eigen::AngleAxisf(-0.03, Eigen::Vector3f::UnitX()));
    pcl::transformPointCloud(*cloud, *cloud, transform);

    transform0 = Eigen::Affine3f::Identity();

    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr _rgb_cloudptr(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_trans(new pcl::PointCloud<pcl::PointXYZI>);
        cv::Mat new_image_raw = image.clone(); //免得画在原图上
        {
            for (std::size_t ind = 0; ind < cloud->points.size(); ind++)
            {
                //对待检测区域(base_link)内的点进行筛选
                if (cloud->points[ind].x < 1 ||
                    cloud->points[ind].x > 80 ||
                    cloud->points[ind].y < -30 ||
                    cloud->points[ind].y > 30 ||
                    cloud->points[ind].z < -5 ||
                    cloud->points[ind].z > 10)
                    continue;

                pcl::PointXYZI itA;
                //外参
                // std::cout << "\n内参\n"
                //           << _i_params.camera_mat << "\n畸变\n"
                //           << _i_params.dist_coeff << std::endl;
                itA = pcl::transformPoint(cloud->points[ind], transform0);
                cloud_trans->push_back(itA);
                //内参+畸变
                cv::Point2d img_pts = converto_imgpts(itA);
                if (img_pts.x < 0 || img_pts.x > image.cols || img_pts.y < 0 ||
                    img_pts.y > image.rows)
                    continue; //判断点是否在图像之外
                cv::circle(new_image_raw, cv::Point(img_pts.x, img_pts.y), 3,
                           CV_RGB(255 * 0.5, 255 * 0.5, 255 * 0.3), -1);

                pcl::PointXYZRGB pt;
                pt.x = cloud->points[ind].x;
                pt.y = cloud->points[ind].y;
                pt.z = cloud->points[ind].z;
                pt.b = image.at<cv::Vec3b>(img_pts.y, img_pts.x)[0];
                pt.g = image.at<cv::Vec3b>(img_pts.y, img_pts.x)[1];
                pt.r = image.at<cv::Vec3b>(img_pts.y, img_pts.x)[2];
                _rgb_cloudptr->push_back(pt);
            }
        }
        pubpc("cloud0", *_rgb_cloudptr);
        pubpc("cloud", *cloud);
        pubpc("cloud_trans", *cloud_trans);
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", new_image_raw).toImageMsg();
        pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
